perm filename CAMERA.SAI[SYS,HE] blob
sn#105010 filedate 1974-06-04 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00022 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 BEGIN "CAMERA"
C00007 00003 \ pot limits and misc. procedures
C00010 00004 \ PRINT LIMITS
C00013 00005 \ READS DATA IN GILL'S FORMAT
C00015 00006 \ SERVO THE SELECTED CAMERA
C00018 00007 \ Compute the transformation matrices for the camera model
C00020 00008 \ CONTINUE GENERATING TRANSFORM
C00023 00009 \ UPDATE THE TRANSFORM - get readings for COHU
C00026 00010 \ UPDATE THE TRANSFORM - get readings for Sierra and process
C00030 00011 \ STILL MORE UPDATING
C00033 00012 \ FINISH UPDATE
C00036 00013 \ ERROR CHECKING ROUTINES
C00039 00014 \ CHANGE LENS OR MOVE PAN/TILT HEAD
C00042 00015 \ CHANGE LENS FOCUS AND ZOOM
C00044 00016 \ CHANGE IRIS
C00045 00017 \ CALCULATIONS COMMON TO PREDICTION AND CENTERING
C00047 00018 \ CENTER CAMERA
C00052 00019 \ TRANSFORM PREDICTION AND PROGRAM INITIALIZATION
C00057 00020 \ PROGRAM TEST ROUTINE
C00059 00021 \ CONTINUE TESTING
C00061 00022 \ THIS IS THE MAIN PROGRAM
C00063 ENDMK
C⊗;
BEGIN "CAMERA"
REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
REQUIRE "HELIB[1,3]" LIBRARY;
REQUIRE "SOBMAT[SYS,HE]" LOAD_MODULE;
REQUIRE -1 NEW_ITEMS;
REQUIRE "⊂⊃||" DELIMITERS;
REQUIRE "TSERVO[LIB,HE]" LOAD_MODULE;
REQUIRE "SSERVO[LIB,HE]" LOAD_MODULE;
DEFINE \=⊂COMMENT⊃,
SAFEX=⊂SAFE⊃,
XDATA=⊂3⊃,
CRLF=⊂&'15&'12⊃,
YES=⊂INCHWL="Y"⊃,
RETYPE=⊂OUTSTR(" ... type Y<cr> to retry ")⊃,
KY=⊂684.5⊃,
OCT(A)=⊂" A="&CVOS(A)⊃,
INT(A)=⊂" A="&CVS(A)⊃,
FLT(A)=⊂" A="&CVF(A)⊃,
SAVE=⊂CAMERR[CAMNUM]←CAMFLG⊃,
DEG=⊂57.29578⊃,
ERROR(I)=⊂CAMFLG←CAMFLG LOR (I)⊃,
CAMERA(I)=⊂ IF ¬(1≤(CAMNUM←I)≤2) THEN
BEGIN ERROR('10000); SAVE; RETURN; END;
CAMJOB[CAMNUM] ← JOB;
IF I=2 THEN CAMLENS←5⊃;
\ ERRORS: 1-SERVO PROBLEMS(SERVO).
2-NOT ENOUGH READINGS(CAM_UPDATE).
4-POTS TOO NOISY(CAM_UPDATE).
10-LENS OUT OF BOUNDS(CHNG_LENS,CAM_CENTER).
20-PAN OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
40-TILT OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
100-FOCUS OUT OF BOUNDS(CHNG_FOCUS,CAM_CENTER).
200-ZOOM OUT OF BOUNDS
400-IRIS OUT OF BOUNDS
1000-LENS CHANGER GOOFED(CHNG_LENS).
2000-LOOKUP FAILED(DATXFR).
10000-ILLEGAL CAMERA NUMBER
20000-AD NOT AVAILABLE ;
SHORT REAL TILPOT, FOCPOT, ZOOPOT, PANPOT, CAMPAN, CAMTIL, CAMRANG, C1, C2,
CAMZOOM, IPOT, ZPOT0, ZPOTD, REF, FOC_LGTH, RANG, PAN, TILT;
SHORT INTEGER MESS, BLOCK, CAMNUM, CAMLENS, CAMFLG, JOB;
EXTERNAL SHORT REAL L1, L2, L3, L4, L5, L6, L7, L8, E1, E2, E3, P1,
P2, P3, P4, P5, P6, P7, P8, SREF, CREF;
EXTERNAL SHORT INTEGER LENS, STATUS, TSERVO, STATE, SSERVO, POT, TVCAM,
FLINE, LLINE, RSIDE, LSIDE, TVWORD;
SAFEX REAL ARRAY MCOL,MICOL [1:3,1:3]; \ WHERE I KEEP CURRENT CAMERA XFORM;
SAFEX REAL ARRAY LCEN[1:3]; \ AND LENS CENTER ;
SAFEX REAL ARRAY MODEL, PREDIC[1:10,1:3]; \ THESE ARE THE MODELS;
\ pot limits and misc. procedures;
EXTERNAL PROCEDURE TELL(INTEGER DEVICE);
EXTERNAL PROCEDURE CALLEN;
EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
EXTERNAL PROCEDURE SPWOFF;
EXTERNAL PROCEDURE INVRT(REAL ARRAY A,AI);
EXTERNAL PROCEDURE TVIN;
EXTERNAL INTEGER PROCEDURE GIOWD(INTEGER ARRAY FOO);
\ arrays to hold calibration constants;
SAFEX REAL ARRAY PPOT0, PPOTD, TPOT0, TPOTD, FPOT0, FPOTD, MART, SWING,
GROREF, FOC, FOCLEN0, FOCLENG[1:5], DP, P0[1:5,1:3], PP[1:5,1:2];
\ pot limits;
preload_with 2020.,1850.; SAFEX real array plimit1[1:2];
preload_with -2020.,154.; SAFEX real array plimit2[1:2];
preload_with 2030.,2030.; SAFEX real array tlimit1[1:2];
preload_with -1850.,-2000.; SAFEX real array tlimit2[1:2];
preload_with 2000.,1800.; SAFEX real array flimit1[1:2];
preload_with -1720.0,-1425.; SAFEX real array flimit2[1:2];
preload_with -2048.,-790.; safex real array ilim1[1:2];
preload_with 2048.,1630.; safex real array ilim2[1:2];
define zlim1=⊂-1700.0⊃, zlim2=⊂1740.0⊃;
\ Solves the eq. Acos(x)+Bsin(x)+C=0 for x;
SIMPLE REAL PROCEDURE COSQR(REAL A,B,C);
BEGIN REAL K,M,N,Y,Z;
K←A*C;
M←A↑2+B↑2;
N←B↑2-C↑2;
Y ← K/M;
Z ← SQRT(Y↑2+(N/M));
RETURN(ACOS(-Y+(IF B<0 THEN Z ELSE -Z)));
END "COSQR";
SIMPLE REAL PROCEDURE LNS(REAL X; INTEGER RLENS);
RETURN (X*FOC[RLENS]/(X-FOC[RLENS]));
\ PRINT LIMITS;
SIMPLE PROCEDURE LIMPRT;
BEGIN INTEGER I, J, K;
REAL VAR1, VAR2, CON1, CON2;
SETFORMAT(1,0);
OUTSTR(NULL CRLF&"CAMERA "&CVS(CAMNUM)&" :"CRLF);
J ← IF CAMNUM=1 THEN 1 ELSE 5;
I ← IF CAMNUM=1 THEN 4 ELSE 5;
FOR CAMLENS ← J STEP 1 UNTIL I DO
BEGIN "LENS"
SETFORMAT(1,0);
OUTSTR(" LENS "&CVS(CAMLENS)&" :"CRLF);
SETFORMAT(7,2);
CON1 ← FOCLEN0[CAMLENS];
CON2 ← FOCLENG[CAMLENS];
VAR1 ← LNS(CON1+CON2*FLIMIT2[CAMNUM],CAMLENS);
VAR2 ← LNS(CON1+CON2*FLIMIT1[CAMNUM],CAMLENS);
OUTSTR(" FOCUS RANGE IS "&CVF(VAR2)&" TO "&CVF(VAR1)&
" INCHES"CRLF);
CON1 ← PPOTD[CAMLENS];
CON2 ← PPOT0[CAMLENS];
VAR1 ← (CON1*PLIMIT2[CAMNUM]+CON2)*DEG;
VAR2 ← (CON1*PLIMIT1[CAMNUM]+CON2)*DEG;
OUTSTR(" PAN RANGE IS "&CVF(VAR2)&" TO "&CVF(VAR1)&
" DEGREES"CRLF);
CON1 ← TPOTD[CAMLENS];
CON2 ← TPOT0[CAMLENS];
VAR1 ← (CON1*TLIMIT2[CAMNUM]+CON2)*DEG;
VAR2 ← (CON1*TLIMIT1[CAMNUM]+CON2)*DEG;
IF CAMNUM=1 THEN VAR1↔VAR2;
OUTSTR(" TILT RANGE IS "&CVF(VAR1)&" TO "&CVF(VAR2)&
" DEGREES"CRLF);
IF CAMNUM=2 THEN
BEGIN "ZOOM"
OUTSTR(" ZOOM RANGE IS ");
OUTSTR(CVF((C1*ZLIM2+C2)*25.4));
OUTSTR(" TO "&CVF((C1*ZLIM1+C2)*25.4)&" MM" CRLF);
END "ZOOM";
END "LENS";
END "LIMPRT";
\ READS DATA IN GILL'S FORMAT;
SIMPLE PROCEDURE DATXFR;
BEGIN
BOOLEAN FLAG;
DEFINE XFR(X)=⊂ARRYIN(XDATA,X,1)⊃,
DATASET= ⊂(IF CAMNUM=1 THEN "DATA1[CAL,HE]"
ELSE "DATA2[CAL,HE]")⊃;
OPEN(XDATA,"DSK",12,3,0,0,0,0);
LOOKUP(XDATA,DATASET,FLAG);
IF FLAG THEN
BEGIN
OUTSTR("DATXFR-FAILED: LOOKUP FAILED FOR "&DATASET CRLF);
ERROR('2000);
RETURN;
END;
IF TYP_CAM THEN OUTSTR("DATXFR: RETRIEVING "&DATASET&CVS(BLOCK)CRLF);
USETI(XDATA,1);
WORDIN(XDATA);
USETI(XDATA,BLOCK);
IF CAMNUM=2 THEN
BEGIN BLOCK←5; XFR(ZPOT0); XFR(ZPOTD); END;
XFR(PPOT0[BLOCK]);
XFR(PPOTD[BLOCK]);
XFR(TPOT0[BLOCK]);
XFR(TPOTD[BLOCK]);
XFR(FPOT0[BLOCK]);
XFR(FPOTD[BLOCK]);
XFR(MART[BLOCK]);
XFR(SWING[BLOCK]);
XFR(|PP[BLOCK,1]|);
XFR(|PP[BLOCK,2]|);
XFR(|P0[BLOCK,1]|);
XFR(|P0[BLOCK,2]|);
XFR(|P0[BLOCK,3]|);
XFR(|DP[BLOCK,1]|);
XFR(|DP[BLOCK,2]|);
XFR(|DP[BLOCK,3]|);
XFR(FOC[BLOCK]);
XFR(FOCLEN0[BLOCK]);
XFR(FOCLENG[BLOCK]);
XFR(GROREF[BLOCK]);
IF CAMNUM=2 THEN
BEGIN
WORDIN(XDATA);
XFR(C1);
XFR(C2);
END;
RELEASE(XDATA);
END "DATXFR";
\ SERVO THE SELECTED CAMERA;
SIMPLE PROCEDURE SERVO;
BEGIN INTEGER I, EOF, STATSAV;
LABEL LOPEN, START, NOAD, NOSERV;
STATSAV ← STATUS;
START: STATUS ← STATSAV;
IF CAMNUM=1 THEN
BEGIN "CAM1SR"
SPWON(1,TSERVO);
WHILE ¬(STATUS LAND 1) DO;
SPWOFF;
IF (STATUS LAND '667777)='110000 THEN
NOAD: BEGIN "NOAD1"
OUTSTR("SERVO: ");
TELL(CVSIX("AD"));
RETYPE;
IF YES THEN GO START ELSE
BEGIN ERROR(20000);RETURN;END;
END "NOAD1";
IF STATUS LAND '17700 THEN
BEGIN "SR1FAL"
OUTSTR("TSERVO FAILED:"&OCT(STATUS));
NOSERV: RETYPE;
IF YES THEN GO START ELSE
BEGIN ERROR(1);RETURN;END;
END "SR1FAL";
IF DEB_CAM THEN OUTSTR("TSERVO:"&FLT(L1)&FLT(L2)&
FLT(L3)CRLF&FLT(P1)&FLT(P2)&FLT(P3)CRLF);
END "CAM1SR" ELSE
IF CAMNUM=2 THEN
BEGIN "CAM2SR" INTEGER ARG; DEFINE SPCWGO=⊂'047000400003⊃;
I ← 20;
LOPEN: OPEN(1,"AD",'417,0,0,0,0,EOF←TRUE);
IF EOF THEN IF (I←I-1) THEN
BEGIN CALL(1,"SLEEP"); GO LOPEN; END ELSE GO NOAD;
STATE ← POT ← 0;
ARG ← '240000000000+LOCATION(SSERVO);
START_CODE MOVE 1,ARG; SPCWGO 1,; END;
WHILE ¬STATE DO;
SPWOFF;
IF STATE>1 THEN
BEGIN "SR2FAL"
OUTSTR("SSERVO FAILED: "&OCT(STATE));
GO NOSERV;
END "SR2FAL";
IF DEB_CAM THEN OUTSTR("TSERVO: "
&FLT(L4)&FLT(L5)&FLT(L6)&FLT(L7)&FLT(L8)CRLF
&FLT(P4)&FLT(P5)&FLT(P6)&FLT(P7)&FLT(P8)CRLF);
END "CAM2SR";
END "SERVO";
\ Compute the transformation matrices for the camera model
given by LENS and the last pots readings;
PROCEDURE PANTIL_CAM(INTEGER L;REAL PPOT,TPOT,FPOT,ZPOT;
SAFEX REAL ARRAY COL,ICOL,CENTER);
BEGIN INTEGER I,J;
REAL ACC,FMX,FMY,PAN,TILT, A, B, CC, D, A1, A2, A3, A4, A5;
SAFEX REAL ARRAY RP,RT,RPT,RS,R[1:3,1:3],C[1:3];
PAN ← PPOTD[L]*PPOT+PPOT0[L];
TILT ← TPOTD[L]*TPOT+TPOT0[L];
FMY ← FPOTD[L]*FPOT+FPOT0[L];
IF CAMNUM=2 THEN FMY ← FMY+ZPOTD/(ZPOT-ZPOT0);
FMX ← FMY*MART[L];
RP[2,3] ← -1;
RPT[1,1] ← RP[1,1] ← RP[3,2] ← B ← -SIN(PAN);
RP[3,1] ← A ← -(RPT[1,2] ← RP[1,2] ← COS(PAN));
RT[1,1] ← RS[3,3] ← 1;
RPT[2,3] ← D ← -(RT[2,2] ← RT[3,3] ← COS(TILT));
R[3,3] ← RPT[3,3] ← RT[2,3] ← CC ← -(RT[3,2] ← SIN(TILT));
RPT[2,1] ← A3 ← CC*A;
RPT[2,2] ← A4 ← CC*B;
R[3,1] ← RPT[3,1] ← -D*A;
R[3,2] ← RPT[3,2] ← -D*B;
RS[1,1] ← RS[2,2] ← A1 ← COS(A2 ← SWING[L]);
RS[2,1] ← A5 ← -(RS[1,2] ← SIN(A2));
R[1,1] ← A1*B-A5*A3;
R[1,2] ← -A1*A-A5*A4;
R[1,3] ← -A5*D;
R[2,1] ← A5*B+A1*A3;
R[2,2] ← -A5*A+A1*A4;
R[2,3] ← A1*D;
A ← DP[L,1];
B ← DP[L,2];
CC ← DP[L,3];
FOR I←1 STEP 1 UNTIL 3 DO
C[I]←P0[L,I]+RPT[1,I]*A+RPT[2,I]*B+RPT[3,I]*CC;
\ CONTINUE GENERATING TRANSFORM;
\ -FMX/333 and PP[2]-PP[1]/333 are corrections for
the non-orthogonality of the TV scan axes;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
COL[I,1]←R[I,1];
COL[I,2]←R[I,2];
ACC←0;
FOR J←1 STEP 1 UNTIL 3 DO ACC←ACC-R[I,J]*C[J];
COL[I,3]←ACC;
END;
A ← FMX/333.0;
B ← PP[L,2]-(CC←PP[L,1])/333.0;
FOR J←1 STEP 1 UNTIL 3 DO
BEGIN
COL[2,J]←-A*COL[1,J]+FMY*COL[2,J]+B*COL[3,J];
COL[1,J]←FMX*COL[1,J]+CC*COL[3,J];
END;
INVRT(COL,ICOL);
ARRTRAN(CENTER,C);
END "PANTIL_CAM";
\ UPDATE THE TRANSFORM - get readings for COHU;
SIMPLE MESSAGE PROCEDURE CAM_UPDATE(INTEGER CAMNO);
BEGIN
LABEL ETA, NOAD;
SHORT REAL SFOC, STIL, SPAN, IND, FMAX, FMIN, TMAX, TMIN, PMAX,
PMIN, SZOM, ZMAX, ZMIN, DIFFOC, DIFTIL, DIFPAN, DIFZOM,
SIND, IMAX, IMIN, DIFIRIS ,SIRIS, DP, DT, DF, DZ, DI;
INTEGER I;
CAMERA(CAMNO);
ETA: SFOC ← SPAN ← STIL ← SZOM ← SIRIS ← 0;
FMAX ← TMAX ← PMAX ← ZMAX ← IMAX ← -10000;
FMIN ← TMIN ← PMIN ← ZMIN ← IMIN ← 10000;
IF CAMNO=1 THEN
BEGIN "CM1UPD"
IMAX ← IMIN ← 0;
STATUS←1;
SPWON(1,TSERVO);
I ← '44;
FOR IND←0 STEP 1 UNTIL 19 DO
BEGIN "CM1LOP"
STATUS←I;
I ← 4;
WHILE ¬(STATUS LAND 1) DO;
IF (STATUS LAND '667777)='110000 THEN
NOAD: BEGIN "NOAD2"
SPWOFF;
OUTSTR("CAM_UPDATE: ");
TELL(CVSIX("AD"));
RETYPE;
IF YES THEN GO ETA ELSE
BEGIN ERROR(20000);RETURN;END;
END "NOAD2";
IF STATUS LAND '17700 THEN
BEGIN "SR3FAL"
OUTSTR("TSERVO FAILED:"&OCT(STATUS)CRLF);
CONTINUE;
END "SR3FAL";
SFOC←SFOC+P1;
STIL←STIL+P2;
SPAN←SPAN+P3;
IF P1>FMAX THEN FMAX←P1;
IF P1<FMIN THEN FMIN←P1;
IF P2>TMAX THEN TMAX←P2;
IF P2<TMIN THEN TMIN←P2;
IF P3>PMAX THEN PMAX←P3;
IF P3<PMIN THEN PMIN←P3;
END "CM1LOP";
SPWOFF;
CAMLENS ← LENS+1;
END "CM1UPD" ELSE
\ UPDATE THE TRANSFORM - get readings for Sierra and process;
IF CAMNO=2 THEN
BEGIN "CM2UPD"
FOR IND←0 STEP 1 UNTIL 19 DO
BEGIN "CM2LOP"
STATE ← 0;
POT ← 1;
SPWON(0,SSERVO);
WHILE ¬STATE DO;
SPWOFF;
IF STATE LAND '100 THEN GO NOAD;
IF ¬(STATE=1) THEN
BEGIN "SR4FAL"
OUTSTR("SSERVO FAILED:"&OCT(STATE)CRLF);
CONTINUE;
END "SR4FAL";
SPAN←SPAN+P4;
SFOC←SFOC+P6;
STIL←STIL+P5;
SZOM←SZOM+P7;
SIRIS←SIRIS+P8;
IF P4>PMAX THEN PMAX←P4;
IF P4<PMIN THEN PMIN←P4;
IF P5>TMAX THEN TMAX←P5;
IF P5<TMIN THEN TMIN←P5;
IF P6>FMAX THEN FMAX←P6;
IF P6<FMIN THEN FMIN←P6;
IF P7>ZMAX THEN ZMAX←P7;
IF P7<ZMIN THEN ZMIN←P7;
IF P8>IMAX THEN IMAX←P8;
IF P8<IMIN THEN IMIN←P8;
END "CM2LOP";
END "CM2UPD";
IF IND<10 THEN
BEGIN "RDLOW"
OUTSTR("CAM_UPDATE: NOT ENOUGH READINGS"&INT(IND));
RETYPE;
IF YES THEN GO ETA ELSE ERROR(2);
END "RDLOW";
REF← IF CAMNO=1 THEN CREF ELSE GROREF[5];
FOCPOT←SFOC*REF/IND;
TILPOT←STIL*REF/IND;
PANPOT←SPAN*REF/IND;
IPOT ← SIRIS*REF/IND;
IF CAMNO=2 THEN L7←ZOOPOT←SZOM*REF/IND;
DIFFOC←(FMAX-FMIN)*REF;
DIFTIL←(TMAX-TMIN)*REF;
DIFPAN←(PMAX-PMIN)*REF;
DIFIRIS←(IMAX-IMIN)*REF;
IF CAMNO=2 THEN DIFZOM←(ZMAX-ZMIN)*REF;
\ STILL MORE UPDATING;
IF DEB_CAM THEN OUTSTR("CAM_UPDATE:"
&FLT(FOCPOT)&FLT(TILPOT)&FLT(PANPOT)&FLT(IPOT)
&(IF CAMNO=2 THEN FLT(ZOOPOT) ELSE NULL) CRLF
&INT(DIFFOC)&INT(DIFTIL)&INT(DIFPAN)
&INT(DIFIRIS) CRLF
&(IF CAMNO=2 THEN INT(DIFZOM) ELSE NULL) CRLF);
SIND←4*SQRT(IND);
DZ ← IF CAMNO=2 THEN DIFZOM/SIND ELSE 0;
DF←DIFFOC/SIND;
DT←DIFTIL/SIND;
DP←DIFPAN/SIND;
DI←DIFIRIS/SIND;
IF DI>.75∨DP>.75∨DT>.75∨DF>1∨(CAMNO=2∧DZ>1) THEN
BEGIN "DIFBAD"
OUTSTR("CAM_UPDATE: POTS TOO NOISY "&INT(CAMNUM));
IF DF>1.0 THEN OUTSTR(INT(DIFFOC));
IF DT>.75 THEN OUTSTR(INT(DIFTIL));
IF DP>.75 THEN OUTSTR(INT(DIFPAN));
IF DI>.75 THEN OUTSTR(INT(DIFIRIS));
IF DZ>1.0 THEN OUTSTR(INT(DIFZOM));
OUTSTR(NULL CRLF);
ERROR(4);
END "DIFBAD";
IF (CAMFLG LAND '20007)≠0 THEN
BEGIN "ERROR"
RETYPE;
IF YES THEN
BEGIN "RETRY"
CAMFLG←CAMFLG LAND '777777757770;
GOTO ETA
END "RETRY" ELSE
IF CAMFLG LAND '20000 THEN
BEGIN "ADERR"
OUTSTR("CAM_UPDATE-FAILED: CAMERA_MODEL NOT"&
" UPDATED"CRLF);
SAVE;
RETURN;
END "ADERR";
END "ERROR";
\ NOW WE HAVE A GOOD SET OF POT READINGS, UPDATE THE TRANSFORM;
PANTIL_CAM(CAMLENS,PANPOT,TILPOT,FOCPOT,ZOOPOT,MCOL,MICOL,LCEN);
\ FINISH UPDATE;
IF CAMNO=2 THEN
BEGIN "CM2FIX"
FOC[5]←C1+ZOOPOT*C2;
FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
FOCLENG[5]←FPOTD[5]/KY;
L4←P4;
L5←P5;
L6←P6;
L6←P7;
L8←P8;
END "CM2FIX";
REF ← IF CAMNUM THEN CREF ELSE SREF;
POTS[1,CAMNUM] ← PANPOT/REF;
POTS[2,CAMNUM] ← TILPOT/REF;
POTS[3,CAMNUM] ← FOCPOT/REF;
POTS[4,CAMNUM] ← IF CAMNUM THEN ZOOPOT/REF ELSE 0;
POTS[5,CAMNUM] ← IPOT/REF;
\ Now to update the global model;
ARRBLT (MODEL[1,1],MCOL[1,1],9);
ARRBLT (MODEL[6,1],MICOL[1,1],9);
ARRBLT (MODEL[4,1],LCEN[1],3);
MODEL[5,1] ← PP[CAMLENS,1];
MODEL[5,2] ← PP[CAMLENS,2];
MODEL[5,3] ← 1.0;
MODEL[9,1] ← CAMPAN ← (PPOTD[CAMLENS]*PANPOT+PPOT0[CAMLENS])*DEG;
MODEL[9,2] ← CAMTIL ← (TPOTD[CAMLENS]*TILPOT+TPOT0[CAMLENS])*DEG;
MODEL[9,3] ← CAMRANG ← LNS(FOCLEN0[CAMLENS]+FOCLENG[CAMLENS]*
FOCPOT,CAMLENS);
MODEL[10,3] ← IPOT;
MODEL[10,2] ← CAMZOOM ← IF CAMNUM=1 THEN CAMLENS ELSE FOC[5]*25.4;
MODEL[10,1]←CAMNO;
CURCAM[camno] ← GLOBAL NEW (MODEL);
IF DEB_CAM THEN OUTSTR("CAM_UPDATE: "&FLT(CAMPAN)&" DEGREES"CRLF&
FLT(CAMTIL)&" DEGREES"CRLF&
FLT(CAMRANG)&" INCHES"CRLF&
FLT(IPOT)&" UNITS"CRLF&
(IF CAMNO=2 THEN FLT(CAMZOOM)&" MM" ELSE NULL) CRLF);
SAVE;
END "CAM_UPDATE";
\ ERROR CHECKING ROUTINES;
SIMPLE BOOLEAN PROCEDURE BADFOC(REAL LL1; STRING PROC);
IF ¬(FLIMIT2[CAMNUM]≤LL1≤FLIMIT1[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": FOCUS OUT OF BOUNDS, READING="&CVF(LL1)CRLF);
ERROR('100);
RETURN(TRUE);
END ELSE RETURN(FALSE);
SIMPLE PROCEDURE BADTIL(REAL LL2; STRING PROC);
IF ¬(TLIMIT2[CAMNUM]≤LL2≤TLIMIT1[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": TILT OUT OF BOUNDS, READING="&CVG(LL2)CRLF);
ERROR('40);
END;
SIMPLE PROCEDURE BADPAN(REAL LL3; STRING PROC);
IF ¬(PLIMIT2[CAMNUM]≤LL3≤PLIMIT1[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": PAN OUT OF BOUNDS, READING="&CVF(LL3)CRLF);
ERROR('20);
END;
SIMPLE PROCEDURE BADLEN(INTEGER RLENS; STRING PROC);
BEGIN
SERVO;
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
OUTSTR(PROC&": I GOOFED, NOW YOU HAVE"& " LENS NO. "&
CVS(LENS+1)CRLF);
ERROR('1000);
END;
END;
SIMPLE BOOLEAN PROCEDURE LENBND(INTEGER RLENS; STRING PROC);
IF ¬(1≤RLENS≤4) THEN
BEGIN
OUTSTR(PROC&": LENS NO. OUT OF BOUNDS -"&CVS(RLENS)CRLF);
ERROR('10);
RETURN(TRUE);
END ELSE RETURN(FALSE);
SIMPLE PROCEDURE BADIRIS(REAL LL3; STRING PROC);
IF ¬(ILIM1[CAMNUM]≤LL3≤ILIM2[CAMNUM]) THEN
BEGIN
OUTSTR(PROC&": IRIS OUT OF BOUNDS, READING="&CVF(LL3)CRLF);
ERROR('400);
END;
SIMPLE PROCEDURE BADZOOM(REAL LL7; STRING PROC);
IF ¬(ZLIM1≤LL7≤ZLIM2) THEN
BEGIN
OUTSTR(PROC&": ZOOM OUT OF BOUNDS, READING="&CVF(LL7)CRLF);
ERROR('200);
END;
\ CHANGE LENS OR MOVE PAN/TILT HEAD;
SIMPLE MESSAGE PROCEDURE CHNG_LENS(INTEGER RLENS);
BEGIN
CAMERA(1);
IF ¬LENBND(RLENS,"CHNG_LENS") THEN
BEGIN
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
LENS←RLENS-1;
STATUS←'20;
BADLEN(RLENS,"CHNG_LENS");
END;
CAMLENS←LENS+1;
CAM_UPDATE(1);
END;
SAVE;
END "CHNG_LENS";
SIMPLE MESSAGE PROCEDURE MOVE_CAM(INTEGER CAMNO;REAL PAN,TILT);
BEGIN REAL LL2,LL3;
CAMERA(CAMNO);
ref← if camno=1 then cref else GROREF[5];
PAN ← PAN/DEG;
TILT ← TILT/DEG;
LL3←(PAN-PPOT0[CAMLENS])/PPOTD[CAMLENS];
LL2←(TILT-TPOT0[CAMLENS])/TPOTD[CAMLENS];
IF CAMNO=1 THEN
BEGIN
L3←LL3/REF;
L2←LL2/REF;
L1←P1;
END ELSE BEGIN
L4←LL3/REF;
L5←LL2/REF;
L6←P6;
L7←P7;
L8←P8;
END;
BADTIL(LL2,"MOVE_CAM");
BADPAN(LL3,"MOVE_CAM");
IF CAMFLG=0 THEN
BEGIN
STATUS←'10;
SERVO;
CAM_UPDATE(camno);
END ELSE SAVE;
END "MOVE_CAM";
\ CHANGE LENS FOCUS AND ZOOM;
SIMPLE MESSAGE PROCEDURE CHNG_FOCUS(INTEGER CAMNO;REAL RANG);
BEGIN REAL LL1;
CAMERA(CAMNO);
ref← if camno=1 then cref else GROREF[5];
IF CAMNO=2 THEN
BEGIN
FOCLENG[5]←FPOTD[5]/KY;
FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
L4←P4;
L5←P5;
L7←P7;
L8←P8;
END;
LL1←(LNS(RANG,CAMLENS)-FOCLEN0[CAMLENS])/FOCLENG[CAMLENS];
IF BADFOC(LL1,"CHNG_FOCUS") THEN BEGIN SAVE;RETURN;END;
IF CAMNO=1 THEN L1←LL1/REF ELSE L6←LL1/REF;
L2←P2;
L3←P3;
STATUS←'10;
SERVO;
CAM_UPDATE(camno);
END "CHNG_FOCUS";
SIMPLE MESSAGE PROCEDURE CHNG_ZOOM(REAL FOC_LGTH);
BEGIN "CHNG_ZOOM" REAL LL7;
CAMERA(2);
L4←P4;
L5←P5;
L6←P6;
L8←P8;
LL7←(FOC_LGTH/25.4-C1)/C2;
L7 ← LL7/GROREF[5];
BADZOOM(LL7,"CHNG_ZOOM");
IF ¬CAMFLG THEN
BEGIN
SERVO;
CAM_UPDATE(2);
END;
END "CHNG_ZOOM";
\ CHANGE IRIS;
SIMPLE MESSAGE PROCEDURE CHNG_IRIS(INTEGER CAMNO; REAL IRIS);
BEGIN "CHNG_IRIS"
CAMERA(CAMNO);
REF ← IF CAMNO=1 THEN CREF ELSE GROREF[5];
IF CAMNO=1 THEN
BEGIN "CAM1IR"
OUTSTR("NO IRIS ON COHU YET" CRLF);
ERROR('400);
RETURN;
END "CAM1IR";
BADIRIS(IRIS,"CHNG_IRIS");
IF ¬CAMFLG THEN
BEGIN
L8 ← IRIS;
L7 ← P7;
L6 ← P6;
L5 ← P5;
L4 ← P4;
SERVO;
CAM_UPDATE(CAMNO);
END;
END "CHNG_IRIS";
\ CALCULATIONS COMMON TO PREDICTION AND CENTERING;
SIMPLE PROCEDURE CALC(INTEGER RLENS; REAL XTC,YTC,ZTC;REFERENCE REAL LL1,
LL2,LL3,LL4,ZOOM; STRING NAME);
BEGIN
REAL XCC,YCC,ZCC,D;
XCC ← P0[RLENS,1]-XTC;
YCC ← P0[RLENS,2]-YTC;
ZCC ← P0[RLENS,3]-ZTC;
ZOOM ← ZOOM/25.4;
PAN ← COSQR(YCC,-XCC,DP[RLENS,1]);
IF (CAMNUM=2) ∧ (P0[RLENS,2]+DP[RLENS,1]-YTC<0) THEN PAN ← -PAN;
D ← XCC*COS(PAN)+YCC*SIN(PAN);
TILT ← COSQR(ZCC,-D,-DP[RLENS,2]);
RANG ← D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];
LL4 ← IF ZOOM THEN (ZOOM-C1)/C2 ELSE ZOOPOT;
LL3 ← (PAN-PPOT0[RLENS])/PPOTD[RLENS];
LL2 ← (TILT-TPOT0[RLENS])/TPOTD[RLENS];
LL1 ← (LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
BADTIL(LL2,NAME);
BADPAN(LL3,NAME);
BADFOC(LL1,NAME);
IF CAMNUM=2 THEN BADZOOM(LL4,NAME);
END;
\ CENTER CAMERA;
MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC,ZOOM);
BEGIN REAL LL1,LL2,LL3,LL4;
CAMERA(CAMNO);
ref← if camno=1 then cref else sref;
IF CAMNO=1 ∧ LENBND(RLENS,"CAM_CENTER") THEN BEGIN SAVE; RETURN; END;
CALC(CAMLENS, XTC, YTC, ZTC, LL1, LL2, LL3, LL4, ZOOM, "CAM_CENTER");
IF CAMFLG≠0 THEN BEGIN SAVE; RETURN; END;
IF CAMNO=1 THEN GROREF[CAMLENS]←CREF;
L1←L6←LL1/GROREF[CAMLENS];
L2←L5←LL2/GROREF[CAMLENS];
L3←L4←LL3/GROREF[CAMLENS];
IF CAMNO=2 THEN L7←LL4/GROREF[CAMLENS];
IF CAMNO=1 THEN
BEGIN
CALLEN;
IF LENS≠CAMLENS-1 THEN
BEGIN
LENS←CAMLENS-1;
STATUS←'30;
END ELSE STATUS←'10;
BADLEN(CAMLENS,"CAM_CENTER");
CAMLENS←LENS+1;
END ELSE SERVO;
CAM_UPDATE(camno);
IF DEB_CAM THEN
BEGIN "DEBUG"
SAFE INTEGER ARRAY BUF[1:100];
TVWORD ← GIOWD(BUF);
TVCAM ← CAMNUM;
LSIDE ← PP[CAMLENS,1];
FLINE ← PP[CAMLENS,2];
RSIDE ← LSIDE+10;
LLINE ← FLINE+10;
WHILE INCHRS<0 DO TVIN;
END "DEBUG";
END "CAM_CENTER";
\ TRANSFORM PREDICTION AND PROGRAM INITIALIZATION;
SIMPLE MESSAGE PROCEDURE CAM_PRED(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC,ZOOM);
BEGIN
REAL LL1,LL2,LL3,LL4;
CAMERA(CAMNO);
IF CAMNO=1∧LENBND(RLENS,"CAM_PRED") THEN BEGIN SAVE; RETURN; END ELSE
CAMLENS ← RLENS;
CALC(CAMLENS,XTC,YTC,ZTC,LL1,LL2,LL3,LL4,ZOOM,"CAM_PRED");
IF CAMFLG≠0 THEN BEGIN SAVE; RETURN; END;
PANTIL_CAM (RLENS,LL3,LL2,LL1,LL4,MCOL,MICOL,LCEN);
ARRBLT (PREDIC[1,1],MCOL[1,1],9);
ARRBLT (PREDIC[6,1],MICOL[1,1],9);
ARRBLT (PREDIC[4,1],LCEN[1],3);
PREDIC[5,1] ← PP[CAMLENS,1];
PREDIC[5,2] ← PP[CAMLENS,2];
PREDIC[5,3] ← 1.0;
PREDIC[9,1] ← PAN/DEG;
PREDIC[9,2] ← TILT/DEG;
PREDIC[9,3] ← RANG;
PREDIC[10,1] ← camno;
PREDIC[10,2] ← IF CAMNO=1 THEN CAMLENS ELSE (LL4*C1+C2)/25.4;
PREDIC[10,3] ← CAMFLG;
SAVE;
END "CAM_PRED";
SIMPLE MESSAGE PROCEDURE CAM_INIT(INTEGER CAMNO);
BEGIN
CAMERA(CAMNO);
IF CAMNO=1 THEN
FOR BLOCK←1 STEP 1 UNTIL 4 DO DATXFR;
IF CAMNO=2 THEN
BEGIN
BLOCK←1;
DATXFR;
END;
SAVE;
END"CAM_INIT";
\ PROGRAM TEST ROUTINE;
SIMPLE BOOLEAN PROCEDURE TESTIT;
BEGIN
STRING COORD;
REAL X,Y,Z,NPAN,NTILT,NRANG;
INTEGER NLENS,REQUEST;
STRING ST;
LABEL TES, TEE;
TES: OUTSTR("CAMERA NO.?"CRLF);
CAMNUM ← IF ST←INCHWL="1" THEN 1 ELSE 2;
TEE: OUTSTR("0-EXIT"CRLF&
"1-CHANGE THE LENS"CRLF&
"2-PAN AND TILT THE CAMERA"CRLF&
"3-FOCUS ON A GIVEN RANGE"CRLF&
"4-CENTER THE CAMERA ON A GIVEN POINT"CRLF&
"5-COMPLEMENT DEB_CAM"CRLF&
"6-CHANGE THE ZOOM"CRLF&
"7-UPDATE"CRLF&
"8-PRINT LIMITS"CRLF&
"9-CHANGE IRIS"CRLF);
OUTSTR("...TYPE THE NO. OF THE PROCEDURE YOU WANT TO TEST NOW="CRLF);
REQUEST←CVD(INCHWL);
IF ¬(0≤REQUEST≤9) THEN
BEGIN
OUTSTR("TESTIT-FAILED: ILLEGAL PROCEDURE NO ("&
CVS(REQUEST)&")"CRLF);
GOTO TES;
END;
CAMFLG ← 0;
CASE REQUEST OF
BEGIN
RETURN(FALSE); \ 0;
BEGIN \ 1;
OUTSTR("...TYPE NO. OF LENS="CRLF);
NLENS←CVD(INCHWL);
CHNG_LENS(NLENS);
END;
BEGIN \ 2;
OUTSTR("...TYPE PAN AND TILT IN DEGREES,"&
" EACH FOLLOWED BY C.R="CRLF);
COORD←INCHWL;
NPAN←REALSCAN(COORD,MESS);
COORD←INCHWL;
NTILT←REALSCAN(COORD,MESS);
MOVE_CAM(CAMNUM,NPAN,NTILT);
END;
\ CONTINUE TESTING;
BEGIN \ 3;
OUTSTR("...TYPE RANGE TO FOCUS ON="CRLF);
COORD←INCHWL;
NRANG←REALSCAN(COORD,MESS);
CHNG_FOCUS(CAMNUM,NRANG);
END;
BEGIN \ 4;
OUTSTR("TYPE IN LENS N0. AND X,Y,Z OF CENTER IN TABLE"&
" COORDINATES AND FOCAL LENGTH"
CRLF&"EACH FOLLOWED BY A C.R.="CRLF);
COORD←INCHWL;
NLENS←CVD(COORD);
COORD←INCHWL;
X←REALSCAN(COORD,MESS);
COORD←INCHWL;
Y←REALSCAN(COORD,MESS);
COORD←INCHWL;
Z←REALSCAN(COORD,MESS);
COORD←INCHWL;
FOC_LGTH ← REALSCAN(COORD,MESS);
CAM_CENTER(NLENS,CAMNUM,X,Y,Z,FOC_LGTH);
END;
BEGIN \ 5; DEB_CAM←¬DEB_CAM; GOTO TEE;END;
BEGIN \ 6;
OUTSTR("DESIRED FOCAL LENGTH"CRLF);
COORD←INCHWL;
FOC_LGTH←REALSCAN(COORD,MESS);
CHNG_ZOOM(FOC_LGTH);
END;
BEGIN \ 7;
INTEGER SAV;
SAV ← DEB_CAM; DEB_CAM ← TRUE;
CAM_UPDATE(CAMNUM); DEB_CAM ← SAV;
END;
LIMPRT;
BEGIN \ 9;
OUTSTR("DESIRED IRIS POSITION"CRLF);
COORD ← INCHWL;
CHNG_IRIS(CAMNUM, REALSCAN(COORD,MESS));
END;
END;
RETURN(TRUE);
END "TESTIT";
\ THIS IS THE MAIN PROGRAM;
CAMFLG←0;
TYP_CAM←TRUE;
DEB_CAM←FALSE;
YES_CAM←TRUE;
JOB ← CVSIX("TTY");
FOR MESS ← 1,2 DO
BEGIN
CAMERR[MESS] ← 0;
CAMJOB[MESS] ← JOB;
END;
FOR CAMNUM←1 STEP 1 UNTIL 2 DO
BEGIN
CAM_INIT(CAMNUM);
IF CAMNUM=1 THEN
BEGIN
CALLEN;
CAMLENS←LENS+1;
END ELSE CAMLENS←5;
CAM_UPDATE(CAMNUM);
END;
IF RUN=0 THEN
BEGIN
OUTSTR("...TYPE Y FOR TEST MODE:"CRLF);
IF YES THEN WHILE TESTIT DO;
END ;
PUT_DATA(0,0,"CAM");
OUTSTR("CAM-ACTIVATED"CRLF);
WHILE TRUE DO
BEGIN
MESS ← GET_ENTRY ('120,NULL,"CAM",NULL);
CAMFLG←0;
JOB ← CVSIX(GET_DATA(1,MESS));
MESS←QUEUE('600,MESS);
END;
END "CAMERA";